#define _USE_MATH_DEFINES
#include <cmath>
#include "AlgoEvaluatorModule7NavigationTime.h"

// Helper function to calculate distance between two lat/lon points in meters using Haversine formula.
// TODO: Move this to a common utility file to avoid duplication.
static double haversine_distance(double lat1, double lon1, double lat2, double lon2) {
    const double R = 6371000.0; // Earth radius in meters
    auto to_radians = [](double degree) {
        return degree * M_PI / 180.0;
    };

    double lat1_rad = to_radians(lat1);
    double lon1_rad = to_radians(lon1);
    double lat2_rad = to_radians(lat2);
    double lon2_rad = to_radians(lon2);

    double dlat = lat2_rad - lat1_rad;
    double dlon = lon2_rad - lon1_rad;

    double a = sin(dlat / 2.0) * sin(dlat / 2.0) +
               cos(lat1_rad) * cos(lat2_rad) *
               sin(dlon / 2.0) * sin(dlon / 2.0);
    double c = 2.0 * atan2(sqrt(a), sqrt(1.0 - a));

    return R * c;
}

class algo::AlgoEvaluatorModule7NavigationTimePrivate
{
public:
};

algo::AlgoEvaluatorModule7NavigationTime::AlgoEvaluatorModule7NavigationTime()
	: d(new AlgoEvaluatorModule7NavigationTimePrivate)
{
}

algo::AlgoEvaluatorModule7NavigationTime::~AlgoEvaluatorModule7NavigationTime()
{
	delete d;
}

std::vector<algobase::EvaluateEnvirDataType> algo::AlgoEvaluatorModule7NavigationTime::getNeedType()
{
	return std::vector<algobase::EvaluateEnvirDataType>();
}

int algo::AlgoEvaluatorModule7NavigationTime::exec(const algobase::Path& path, double& result, algobase::EvaluateModuleCoords& indexCoords)
{
	if (path.size() < 2) {
		result = 0.0;
		indexCoords.clear();
		return 0;
	}

	double total_time_seconds = 0.0;
	indexCoords.resize(path.size());
	indexCoords[0].Assign(path[0]);
	indexCoords[0].mRawValue = 0.0; // Time at the start is 0.

	for (size_t i = 0; i < path.size() - 1; ++i) {
		const auto& p1 = path[i];
		const auto& p2 = path[i+1];

		// 3D segment distance calculation
		double h_dist = haversine_distance(p1.mCoordZ[1], p1.mCoordZ[0], p2.mCoordZ[1], p2.mCoordZ[0]);
		double v_dist = std::abs(p2.mCoordZ[2] - p1.mCoordZ[2]);
		double segment_dist = std::sqrt(h_dist * h_dist + v_dist * v_dist);

		// Use the speed at the start of the segment to calculate time for the segment.
		double speed_mps = p1.mSpeed;

		double segment_time_seconds = 0.0;
		if (speed_mps > 1e-6) { // Check for non-zero speed to avoid division by zero.
			segment_time_seconds = segment_dist / speed_mps;
		} else if (segment_dist > 1e-6) {
			// If there is distance to cover but speed is zero, time is effectively infinite.
			// Return a very large number to heavily penalize this path.
			result = 1.0e12; // A large penalty value in hours.
			return 0;
		}
		
		total_time_seconds += segment_time_seconds;

		// Store the INSTANTANEOUS time for the segment at the start point.
		indexCoords[i].Assign(p1);
		indexCoords[i].mRawValue = segment_time_seconds;
		indexCoords[i].mResult = calcScore(segment_time_seconds);
	}
	// Last point has no outgoing segment.
	indexCoords.back().Assign(path.back());
	indexCoords.back().mRawValue = 0.0;

	// The final raw value is the total time in hours.
	result = calcScore(total_time_seconds / 3600.0);

	return 0;
}

double algo::AlgoEvaluatorModule7NavigationTime::calcScore(double rawValue) const
{
	double refTime_s = 1000.0;      // Typical segment time in seconds
	return costSigmoid(rawValue, refTime_s, refTime_s * 0.2) * getWeight();
}

double algo::AlgoEvaluatorModule7NavigationTime::calcRaw(const std::vector<double>& rawValues) const
{
	double total_time_seconds = 0.0;
	for (double time_s : rawValues) {
		total_time_seconds += time_s;
	}
	return total_time_seconds / 3600.0;
}
